/*
 * caosensornode.h
 *
 *
 *  Created on: Oct 8, 2011
 *  Author: Mario H. Voorsluys
 *	email: forcaeluz@gmail.com
 *
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Library General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, visit:
 * http://www.gnu.org/copyleft/gpl.html
 */


#ifndef ROSNODE_H_
#define ROSNODE_H_

// C = Corners
// M = Middle
// S = Side
// B = back

// R = Right
// L = left
#define SONAR_C_R 0xE0
#define SONAR_M_R 0xE2
#define SONAR_M_L 0xE4
#define SONAR_C_L 0xE6
#define SONAR_S_R 0xE8
#define SONAR_S_L 0xEA
#define SONAR_B_R 0xEC
#define SONAR_B_L 0xEE

// Define TWI data and registers for reading and controlling the sonars.
// Data, commands
#define RANGE_CM 		0x51
#define RANGE_IN 		0x50
#define RANGE_MS 		0x52
#define FAKE_RANGE_CM 	0x56
#define FAKE_RANGE_IN 	0x57
#define FAKE_RANGE_MS 	0x58
#define SONAR_BURST 	0x5C
#define FORCE_RESTART 	0x60

//Registers
#define WRITE_COMMAND	0x00
#define READ_HIGH_BYTE	0x02
#define READ_LOW_BYTE	0x03

#include "SerialReader.h"
#include "../msg_gen/cpp/include/cao_board/Sonars.h"
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
//#include <nodemon/state_publisher.h>

class CaoSensorNode {
public:
	/**
	 * Node constructor.
	 * This method actually only calls the init method, and set the usb device name.
	 */
	CaoSensorNode();
	CaoSensorNode(char* file_descriptor);
	/**
	 * Node destructor.
	 * Shuts down this ros node.
	 */
	virtual ~CaoSensorNode();
	/**
	 * Initializes the node. All the publishers are advertized, and the serial communication is started.
	 */
	void init();
	/**
	 * This method is where the node actually works.
	 * The sensor data is read, and published to the channels. Currently error values are interpreted by the SerialReader, and only transmitted
	 * as a -1 value to the node, which emits a ROS_WARNING.
	 */
	void spin();

protected:
	int measurementValue(unsigned char address);

	char* usb_device;
	// The Handle for this node.
	ros::NodeHandle node_handle;
	// The data publisher for the sonars.
	ros::Publisher sonars_publisher;
	// A subscriber, to read information about the robot movement.
	SerialReader communicator;
};

#endif /* ROSNODE_H_ */
